#include "Stalker.h"

Stalker::Stalker() {
	ros::NodeHandle n;
	stalkClient = n.serviceClient<stalker::Stalk> ("stalker_nav");
	trackClient = n.serviceClient<stalker::TrackPos> ("stalker_track");
	subg = n.subscribe("/goal", 100, &Stalker::SetGoal, this);
	subi = n.subscribe("/initialpose", 10, &Stalker::poseCallback, this);
	botPosePub = n.advertise<const geometry_msgs::PoseWithCovarianceStamped> ("botpose", 5);
	manPosePub = n.advertise<geometry_msgs::PoseStamped> ("manpose", 5);
	manPositionInitialized = false;
	botPositionInitialized = false;
	personPose = DD(0.0, 0.0);
}

Stalker::~Stalker() {
}

void Stalker::poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg) {
	ROS_INFO("Got bot pose");
	botPosePub.publish(msg);
	botPositionInitialized = true;
}

void Stalker::SetGoal(const geometry_msgs::PoseStamped &msg) {
	ROS_INFO("Stalking a new person");
	double x = msg.pose.position.x, y = msg.pose.position.y;
	manPosePub.publish(msg);
	manPositionInitialized = true;
}

void Stalker::loop() {
	ros::Rate r(3);
	while (true) {
		boost::this_thread::interruption_point();
		if (botPositionInitialized && trackClient.call(trackService)) {
			std::cout << "IN\n";
			double confidence = trackService.response.confidence, personX =
					trackService.response.personX, personY = trackService.response.personY;
			if (confidence > 0.0 && personX+personY > 1.0 ) {
				STLPose2D currentPose = DD(trackService.response.personX,
						trackService.response.personY);
				/**/
				if (stepSize(personPose, currentPose) > 1.0) {
					personPose = currentPose;
					stalkService.request.goalx = personPose.first;
					stalkService.request.goaly = personPose.second;
					if (stalkClient.call(stalkService)) {
						int success = stalkService.response.success;
						!success ? std::cout << "Stalk next.....\n" : std::cout
								<< "Stalk Not.....\n";
					} else {
						ROS_ERROR("Stalker Nav seems to have failed. Please check if its running");
					}
				}
				/**/
			} else {
				ROS_ERROR("Not confident of person position");
			}
		}

		r.sleep();
	}
}

double Stalker::stepSize(DD pose1, DD pose2) {
	return fabs(pose1.first - pose2.first) + fabs(pose1.second - pose2.second);
}
